#ifndef ICP_H
#define ICP_H

// for Eigen
#include <Eigen/Core>
#include <Eigen/Geometry>
using Eigen::Vector2d;
using Eigen::Vector3d;


// for cv
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc.hpp>
using namespace cv;

// g2o
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

// std 
#include <vector>
#include <list>
#include <memory>
#include <string>
#include <iostream>
#include <set>
#include <unordered_map>
#include <map>
#include <cmath>

// ceres
#include "ceres/ceres.h"

using namespace std;

double distance(const Point3d& a, const Point3d& b);

void pose_estimate_ICP(const vector<cv::Point3d>& A, 
		               vector<cv::Point3d>& B, 
		               Eigen::Matrix3d& R, Eigen::Vector3d& t);
int FindClosedPoint(const vector<Point3d>& B, const Point3d& a);
void rotate_ensemble(const vector<Point3d>& B, vector<Point3d>& C, 
					 const Eigen::Matrix3d& R, const Eigen::Vector3d& t);
#endif
